/*
 * File : gpsfix.c
 *
 */
#include "config.h"
#include "memory.h"
#include "task.h"
#include "tool.h"
#include "gps.h"
#include "gpsfix.h"
#include "message.h"
#include "led.h"
#include "flash_mem.h"
#include "spi_flash.h"
#include <stdlib.h>
#include <math.h>
#include "main.h"

#define EVENT_TIMEOUT	30

__no_init AGPS_INFO	agps_info;
__no_init GPS_FIX_DATA gGPSFix;

GPS_FIX_DATA *pGPSFix = &gGPSFix;

static unsigned short GPSFIX_Time;

GPS_STATUS gps_state;

static double dm2dd(double dm)
{
	double dd;
	int d;
	
	d = (int)dm;
	dd = ((dm - d) * 100 / 60) + d;

	return dd;
}

void UpdateRTC(void)
{
	unsigned long gps_time;
	unsigned long rtc_time;
	DATE_TIME dtime;

    if(pCMD->gpsptcl.mode == 0)
    {
	    dtime.hour	= pGPS->RMC.hour;
	    dtime.min	= pGPS->RMC.min;
	    dtime.sec	= pGPS->RMC.sec;
	    dtime.day	= pGPS->RMC.day;
	    dtime.month	= pGPS->RMC.month;
	    dtime.year	= pGPS->RMC.year + 2000;
    }
    else if(pCMD->gpsptcl.mode == 1)
    {
        dtime.hour	= pUBX->TIMEUTC.hour;
	    dtime.min	= pUBX->TIMEUTC.min;
	    dtime.sec	= pUBX->TIMEUTC.sec;
	    dtime.day	= pUBX->TIMEUTC.day;
	    dtime.month	= pUBX->TIMEUTC.month;
	    dtime.year	= pUBX->TIMEUTC.year;
    }
	
	gps_time = TimeToUnix(dtime);
	rtc_time = RTC_GetCounter();
	
	if (gps_time < UTC_20100101)
		return;
		
	if (abs(gps_time - rtc_time) > 3)
	{
		RTC_SetCounter(gps_time);
		RTC_WaitForLastTask();
		UpdateRTC_Hook();
	}
	
	if (*pFlag_rtc_update == 0)
	{
		Write_Backup_Reg(RTC_UPDATE, 1);
		*pFlag_rtc_update = 1;
	}
}

unsigned char GPS_Calc_Sat_Num(void)
{
	int i;
	unsigned char num = 0;
	for(i = 0; i < pGPS->GSV.sat_num; i++)
	{
		if (pGPS->GSV.sat_info[i].cn > 1)
			num++;
	}
	return num;
}

float gps_speed;

static void GPS_Update_Time(void)
{
    if(pCMD->gpsptcl.mode == 0)
    {
	    if (pGPS->RMC.hour > 23)
		    return;
	    if (pGPS->RMC.min > 59)
		    return;
	    if (pGPS->RMC.sec > 59)
		    return;
	    if (pGPS->RMC.day > 31)
		    return;
	    if (pGPS->RMC.month > 12)
		    return;
	    if (pGPS->RMC.year > 20)
		    return;
    }

	UpdateRTC();
}

static void GPS_Update_Position(void)
{
	int speed;
	
	speed = (int)(pGPS->RMC.speed - gps_speed);
	
	if ((abs(speed) < 28) && (pGPS->RMC.speed < 162))
	{
		/* Acceleration < 14 m/s, Velocity < 300 km/h */
		if ((dm2dd(pGPS->GGA.latitude) == 0) && (dm2dd(pGPS->GGA.longitude) ==  0))
			return;
		gps_speed = pGPS->RMC.speed;
		pGPSFix->time.hour	= pGPS->RMC.hour;
		pGPSFix->time.min	= pGPS->RMC.min;
		pGPSFix->time.sec	= pGPS->RMC.sec;
		pGPSFix->time.day	= pGPS->RMC.day;
		pGPSFix->time.month	= pGPS->RMC.month;
		pGPSFix->time.year	= pGPS->RMC.year + 2000;
		pGPSFix->latitude	= dm2dd(pGPS->GGA.latitude);
		pGPSFix->longitude	= dm2dd(pGPS->GGA.longitude);
		pGPSFix->altitude	= (pGPS->GGA.antenna_h > 0)? pGPS->GGA.antenna_h : 0;
		pGPSFix->speed		= pGPS->RMC.speed;
		pGPSFix->course		= pGPS->RMC.course;
		pGPSFix->hdop		= pGPS->GGA.hdop;
		pGPSFix->sat_num	= pGPS->GGA.sat_num;
		*pFlag_gps_data = 1;
	}
}


unsigned short agps_len;
unsigned long agps_addr;
char agps_send_state;
#define AGPS_SEND_START		1
#define AGPS_SENDING		2
#define AGPS_SEND_END		3

char *agps_buf;

static void AGPS_Send_Task(void)
{
	unsigned short len;
	
	switch (agps_send_state)
	{
	case AGPS_SEND_START:
		DBG_Msg("AGPS Send Data");
		agps_buf = (char *)malloc(sizeof(char) * 256);
		assert_malloc(agps_buf);
		agps_addr = AGPS_ADDR + sizeof(AGPS_INFO);
		agps_send_state = AGPS_SENDING;
		break;
	case AGPS_SENDING:
		if (*pFlag_gps_tx_busy == 0)
		{
			if (agps_len == 0)
			{
				agps_send_state = AGPS_SEND_END;
				return;
			}
			
			if (agps_len > 256)
				len = 256;
			else
				len = agps_len;
			
			SPI_Flash_Read(agps_addr,(unsigned char *)agps_buf, len);
			SPI_Flash_Wait_Ready();
			GPS_Write((unsigned char *)agps_buf, len);
			agps_len -= len;
			agps_addr += len;
		}
		break;
	default:
	case AGPS_SEND_END:
		free(agps_buf);
		assert_free();
		DBG_Msg("AGPS Send Data End");
		*pFlag_gps_agps_fix = 0;
		TaskDel(AGPS_Send_Task);
		break;
	}
}


void AGPS_Send_Data(void)
{
	AGPS_INFO agps;
	
	SPI_Flash_Read(AGPS_ADDR,(unsigned char *)&agps,sizeof(AGPS_INFO));
	SPI_Flash_Wait_Ready();
	
	if (agps.checksum == Calc_CheckSum((char *)&agps, sizeof(AGPS_INFO) - 1))
	{
		agps_len = agps.update_data_len;
		if (TaskAdd(AGPS_Send_Task, (SYS_TICK / SYS_TICK)) == TASK_OK)
		{
			agps_send_state = AGPS_SEND_START;
		}
	}
	else
	{
		*pFlag_gps_agps_fix = 0;
	}
	
}

static void GPS_ACT_OFF(void)
{
	if ((*pFlag_gps_fix != 0) && (AppMode == TRACK_MODE))
	{
		//gps start fix
		DBG_Msg("GPS ON");
		gps_state = GPS_ON;
		pGPS->GSA.fix_type = 0;
		GPS_Power_ON();
		GPS_LED_Flash(1, 1);
		*pFlag_gps_data = 0;
		GPSFIX_Time = 0;
	}
}

static void GPS_ACT_ON(void)
{
	if (pGPS->GSA.fix_type > 1)	
	{
		DBG_Msg("GPS FIX");
		GPS_LED_On();
		GPS_Update_Time();
		pGPS->GSA.fix_type = 0;
		GPSFIX_Time = 0;
		gps_state = GPS_DELAY;
	}
	
	if ((*pFlag_gps_fix == 0) || (AppMode != TRACK_MODE) )	//|| (GPSFIX_Time > pCMD->gpss.timeout))
	{
        if(pCMD->gpsptcl.mode == 0)      //fenris 20101210
		    gps_state = GPS_END;
	}
}

static void GPS_ACT_DELAY(void)
{     
	gps_speed = pGPS->RMC.speed;
	GPS_Update_Time();
	
	if (pGPS->GSA.fix_type == 3)
		if (GPSFIX_Time < 5)
			GPSFIX_Time = 5;
			
	if (GPSFIX_Time > 10)
	{
		GPSFIX_Time = 0;
		gps_state = GPS_FIX;
	}
}

static void GPS_ACT_FIX(void)
{
	if (pGPS->GSA.fix_type > 1)
	{
		pGPS->GSA.fix_type	= 0;
		GPS_Update_Time();
		GPS_Update_Position();
		GPS_FIX_Hook();
		GPSFIX_Time = 0;
	}
	
	if (GPSFIX_Time >= 3)
	{
		*pFlag_gps_data = 0;
		GPSFIX_Time = 0;
		DBG_Msg("GPS NOT FIX");
		GPS_LED_Flash(1, 1);
		gps_state = GPS_NOT_FIX;
	}
	
	if ((*pFlag_gps_fix == 0) || (AppMode != TRACK_MODE))
	{
        if(pCMD->gpsptcl.mode == 0)      //fenris 20101210
		    gps_state = GPS_END;
	}
}

static void GPS_ACT_NOT_FIX(void)
{
	gps_speed = pGPS->RMC.speed;
	
	if (pGPS->GSA.fix_type > 1)
	{
		GPSFIX_Time = 0;
		DBG_Msg("GPS FIX");
		GPS_LED_On();
		gps_state = GPS_FIX;
	}
	
	if ((*pFlag_gps_fix == 0) || (AppMode != TRACK_MODE) )	//|| (GPSFIX_Time > pCMD->gpss.timeout))
	{
        if(pCMD->gpsptcl.mode == 0)      //fenris 20101210
		    gps_state = GPS_END;
	}
}

static void GPS_ACT_END(void)
{
	*pFlag_gps_data = 0;
	DBG_Msg("GPS OFF");
	GPS_LED_Off();
	GPS_Power_OFF();
	gps_state = GPS_OFF;
}

static void (*GPS_Action[])(void) = {
	GPS_ACT_OFF,		// 0
	GPS_ACT_ON,			// 1
	GPS_ACT_DELAY,		// 2
	GPS_ACT_FIX,		// 3
	GPS_ACT_NOT_FIX,	// 4
	GPS_ACT_END,		// 5
};

static void GPS_Fix_Task(void)
{
	GPSFIX_Time++;
	(*GPS_Action[gps_state])();
}

/*
static void AGPS_Task(void)
{
	unsigned long timeout;
	
	if ((pCMD->agps.enb == 0) || (AppMode != TRACK_MODE))
		return;
	
	if (*pFlag_rtc_update == 0)
	{	
		if (*pFlag_agps_update == 0)
			*pFlag_agps_connect = 1;
	}
	else
	{
		if (*pFlag_agps_update == 1)
		{
			*pFlag_agps_update = 0;
			agps_info.update_time = RTC_GetCounter();
			agps_info.checksum = Calc_CheckSum((char *)&agps_info, sizeof(AGPS_INFO) - 1);
			SPI_Flash_Write(AGPS_ADDR, (unsigned char *)&agps_info, sizeof(AGPS_INFO));
		}
		else
		{
			if (*pFlag_agps_connect == 0)
			{
				timeout = RTC_GetCounter() - agps_info.update_time;
				if (timeout > (pCMD->agps.update * 3600))
				{
					DBG_Msg("AGPS Data Timeout");
					*pFlag_agps_connect = 1;
				}
			}
		}
	}
}
*/

void AGPS_Init(void)
{/*
	unsigned short num;
	LOG_MSG *plog;
	
	num = Flash_Queue_Get_num(LOG_FQBUF);
	
	if (agps_info.checksum != Calc_CheckSum((char *)&agps_info, sizeof(AGPS_INFO) - 1))
	{
		agps_info.update_time = 0;
		agps_info.latitude = 0;
		agps_info.longitude = 0;
		agps_info.update_data_len = 0;
	}
	
	// first
	SPI_Flash_Read(AGPS_ADDR,(unsigned char *)&agps_info,sizeof(AGPS_INFO));
	SPI_Flash_Wait_Ready();
	
	if (num == 0)
	{
		pGPSFix->time.year	= 0;
		pGPSFix->time.month	= 0;
		pGPSFix->time.day	= 0;
		pGPSFix->time.hour	= 0;
		pGPSFix->time.min	= 0;
		pGPSFix->time.sec	= 0;
		pGPSFix->latitude	= pCMD->agps.latitude;
		pGPSFix->longitude	= pCMD->agps.longitude;
		pGPSFix->altitude	= 0;
		pGPSFix->speed		= 0;
		pGPSFix->course		= 0;
		pGPSFix->hdop		= 0;
		pGPSFix->sat_num	= 0;
	}
	else
	{
		plog = (LOG_MSG *)malloc(sizeof(LOG_MSG));
		assert_malloc(plog);
		Flash_Queue_Read(LOG_FQBUF, (char *)plog, num, sizeof(LOG_MSG));
		pGPSFix->time.year	= plog->msg.rtc_year + 2000;
		pGPSFix->time.month	= plog->msg.rtc_month;
		pGPSFix->time.day	= plog->msg.rtc_day;
		pGPSFix->time.hour	= plog->msg.rtc_hour;
		pGPSFix->time.min	= plog->msg.rtc_min;
		pGPSFix->time.sec	= plog->msg.rtc_sec;
		pGPSFix->latitude	= (double)plog->msg.latitude / 100000;
		pGPSFix->longitude	= (double)plog->msg.longitude / 100000;
		pGPSFix->altitude	= (float)plog->msg.altitude / 10;
		pGPSFix->speed		= (float)plog->msg.speed / 10;;
		pGPSFix->course		= (float)plog->msg.direction / 10;;
		pGPSFix->hdop		= (float)plog->msg.hdop / 10;;
		pGPSFix->sat_num	= plog->msg.sat_num;
		free(plog);
		assert_free();
	}
	
	if (GPS_Get_Module() == GPS_UBLOX)
		TaskAdd(AGPS_Task, (1000 / SYS_TICK));
*/
}


void GPS_FIX_Init(void)
{
	gps_state = GPS_OFF;
	TaskAdd(GPS_Fix_Task, (1000 / SYS_TICK));
}
